
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_imu_param.c
  * @author     baiyang
  * @date       2021-10-19
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_imu.h"
#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_imu_update_param()
{
    sensor_imu * _imu = sensor_imu_get_singleton();

    _imu->_gyro_offset[0].x = PARAM_GET_FLOAT(INS, INS_GYROFFS_X);
    _imu->_gyro_offset[0].y = PARAM_GET_FLOAT(INS, INS_GYROFFS_Y);
    _imu->_gyro_offset[0].z = PARAM_GET_FLOAT(INS, INS_GYROFFS_Z);
    
    _imu->_gyro_offset[1].x = PARAM_GET_FLOAT(INS, INS_GYR2OFFS_X);
    _imu->_gyro_offset[1].y = PARAM_GET_FLOAT(INS, INS_GYR2OFFS_Y);
    _imu->_gyro_offset[1].z = PARAM_GET_FLOAT(INS, INS_GYR2OFFS_Z);
    
    _imu->_gyro_offset[2].x = PARAM_GET_FLOAT(INS, INS_GYR3OFFS_X);
    _imu->_gyro_offset[2].y = PARAM_GET_FLOAT(INS, INS_GYR3OFFS_Y);
    _imu->_gyro_offset[2].z = PARAM_GET_FLOAT(INS, INS_GYR3OFFS_Z);
    
    _imu->_accel_scale[0].x = PARAM_GET_FLOAT(INS, INS_ACCSCAL_X);
    _imu->_accel_scale[0].y = PARAM_GET_FLOAT(INS, INS_ACCSCAL_Y);
    _imu->_accel_scale[0].z = PARAM_GET_FLOAT(INS, INS_ACCSCAL_Z);
    _imu->_accel_offset[0].x = PARAM_GET_FLOAT(INS, INS_ACCOFFS_X);
    _imu->_accel_offset[0].y = PARAM_GET_FLOAT(INS, INS_ACCOFFS_Y);
    _imu->_accel_offset[0].z = PARAM_GET_FLOAT(INS, INS_ACCOFFS_Z);
    
    _imu->_accel_scale[1].x = PARAM_GET_FLOAT(INS, INS_ACC2SCAL_X);
    _imu->_accel_scale[1].y = PARAM_GET_FLOAT(INS, INS_ACC2SCAL_Y);
    _imu->_accel_scale[1].z = PARAM_GET_FLOAT(INS, INS_ACC2SCAL_Z);
    _imu->_accel_offset[1].x = PARAM_GET_FLOAT(INS, INS_ACC2OFFS_X);
    _imu->_accel_offset[1].y = PARAM_GET_FLOAT(INS, INS_ACC2OFFS_Y);
    _imu->_accel_offset[1].z = PARAM_GET_FLOAT(INS, INS_ACC2OFFS_Z);
    
    _imu->_accel_scale[2].x = PARAM_GET_FLOAT(INS, INS_ACC3SCAL_X);
    _imu->_accel_scale[2].y = PARAM_GET_FLOAT(INS, INS_ACC3SCAL_Y);
    _imu->_accel_scale[2].z = PARAM_GET_FLOAT(INS, INS_ACC3SCAL_Z);
    _imu->_accel_offset[2].x = PARAM_GET_FLOAT(INS, INS_ACC3OFFS_X);
    _imu->_accel_offset[2].y = PARAM_GET_FLOAT(INS, INS_ACC3OFFS_Y);
    _imu->_accel_offset[2].z = PARAM_GET_FLOAT(INS, INS_ACC3OFFS_Z);
    
    _imu->_gyro_filter_cutoff = PARAM_GET_INT16(INS, INS_GYRO_FILTER);
    _imu->_accel_filter_cutoff = PARAM_GET_INT16(INS, INS_ACCEL_FILTER);
    
    _imu->_use[0] = PARAM_GET_INT8(INS, INS_USE);
    _imu->_use[1] = PARAM_GET_INT8(INS, INS_USE2);
    _imu->_use[2] = PARAM_GET_INT8(INS, INS_USE3);

    _imu->_still_threshold = PARAM_GET_FLOAT(INS, INS_STILL_THRESH);
    _imu->_gyro_cal_timing = PARAM_GET_INT8(INS, INS_GYR_CAL);

    _imu->_trim_option = PARAM_GET_INT8(INS, INS_TRIM_OPTION);
    _imu->_acc_body_aligned = PARAM_GET_INT8(INS, INS_ACC_BODYFIX);
    
    _imu->_accel_pos[0].x = PARAM_GET_FLOAT(INS, INS_POS1_X);
    _imu->_accel_pos[0].y = PARAM_GET_FLOAT(INS, INS_POS1_Y);
    _imu->_accel_pos[0].z = PARAM_GET_FLOAT(INS, INS_POS1_Z);
    
    _imu->_accel_pos[1].x = PARAM_GET_FLOAT(INS, INS_POS2_X);
    _imu->_accel_pos[1].y = PARAM_GET_FLOAT(INS, INS_POS2_Y);
    _imu->_accel_pos[1].z = PARAM_GET_FLOAT(INS, INS_POS2_Z);
    
    _imu->_accel_pos[2].x = PARAM_GET_FLOAT(INS, INS_POS3_X);
    _imu->_accel_pos[2].y = PARAM_GET_FLOAT(INS, INS_POS3_Y);
    _imu->_accel_pos[2].z = PARAM_GET_FLOAT(INS, INS_POS3_Z);
    
    _imu->_gyro_id[0] = PARAM_GET_INT32(INS, INS_GYR_ID);
    _imu->_gyro_id[1] = PARAM_GET_INT32(INS, INS_GYR2_ID);
    _imu->_gyro_id[2] = PARAM_GET_INT32(INS, INS_GYR3_ID);
    
    _imu->_accel_id[0] = PARAM_GET_INT32(INS, INS_ACC_ID);
    _imu->_accel_id[1] = PARAM_GET_INT32(INS, INS_ACC2_ID);
    _imu->_accel_id[2] = PARAM_GET_INT32(INS, INS_ACC3_ID);
    
    _imu->_fast_sampling_mask = PARAM_GET_INT8(INS, INS_FAST_SAMPLE);
    _imu->_enable_mask = PARAM_GET_INT8(INS, INS_ENABLE_MASK);
    _imu->_fast_sampling_rate = PARAM_GET_INT8(INS, INS_GYRO_RATE);
}

void sensor_imu_gyr_offset_id_variable_to_param()
{
    sensor_imu * _imu = sensor_imu_get_singleton();

    PARAM_SET_FLOAT(INS, INS_GYROFFS_X, _imu->_gyro_offset[0].x);
    PARAM_SET_FLOAT(INS, INS_GYROFFS_Y, _imu->_gyro_offset[0].y);
    PARAM_SET_FLOAT(INS, INS_GYROFFS_Z, _imu->_gyro_offset[0].z);
    
    PARAM_SET_FLOAT(INS, INS_GYR2OFFS_X, _imu->_gyro_offset[1].x);
    PARAM_SET_FLOAT(INS, INS_GYR2OFFS_Y, _imu->_gyro_offset[1].y);
    PARAM_SET_FLOAT(INS, INS_GYR2OFFS_Z, _imu->_gyro_offset[1].z);
    
    PARAM_SET_FLOAT(INS, INS_GYR3OFFS_X, _imu->_gyro_offset[2].x);
    PARAM_SET_FLOAT(INS, INS_GYR3OFFS_Y, _imu->_gyro_offset[2].y);
    PARAM_SET_FLOAT(INS, INS_GYR3OFFS_Z, _imu->_gyro_offset[2].z);
    
    PARAM_SET_INT32(INS, INS_GYR_ID, _imu->_gyro_id[0]);
    PARAM_SET_INT32(INS, INS_GYR2_ID, _imu->_gyro_id[1]);
    PARAM_SET_INT32(INS, INS_GYR3_ID, _imu->_gyro_id[2]);
    
    param_save();
}

void sensor_imu_acc_scale_offset_id_variable_to_param()
{
    sensor_imu * _imu = sensor_imu_get_singleton();
    
    PARAM_SET_FLOAT(INS, INS_ACCSCAL_X, _imu->_accel_scale[0].x);
    PARAM_SET_FLOAT(INS, INS_ACCSCAL_Y, _imu->_accel_scale[0].y);
    PARAM_SET_FLOAT(INS, INS_ACCSCAL_Z, _imu->_accel_scale[0].z);
    PARAM_SET_FLOAT(INS, INS_ACCOFFS_X, _imu->_accel_offset[0].x);
    PARAM_SET_FLOAT(INS, INS_ACCOFFS_Y, _imu->_accel_offset[0].y);
    PARAM_SET_FLOAT(INS, INS_ACCOFFS_Z, _imu->_accel_offset[0].z);
    
    PARAM_SET_FLOAT(INS, INS_ACC2SCAL_X, _imu->_accel_scale[1].x);
    PARAM_SET_FLOAT(INS, INS_ACC2SCAL_Y, _imu->_accel_scale[1].y);
    PARAM_SET_FLOAT(INS, INS_ACC2SCAL_Z, _imu->_accel_scale[1].z);
    PARAM_SET_FLOAT(INS, INS_ACC2OFFS_X, _imu->_accel_offset[1].x);
    PARAM_SET_FLOAT(INS, INS_ACC2OFFS_Y, _imu->_accel_offset[1].y);
    PARAM_SET_FLOAT(INS, INS_ACC2OFFS_Z, _imu->_accel_offset[1].z);
    
    PARAM_SET_FLOAT(INS, INS_ACC3SCAL_X, _imu->_accel_scale[2].x);
    PARAM_SET_FLOAT(INS, INS_ACC3SCAL_Y, _imu->_accel_scale[2].y);
    PARAM_SET_FLOAT(INS, INS_ACC3SCAL_Z, _imu->_accel_scale[2].z);
    PARAM_SET_FLOAT(INS, INS_ACC3OFFS_X, _imu->_accel_offset[2].x);
    PARAM_SET_FLOAT(INS, INS_ACC3OFFS_Y, _imu->_accel_offset[2].y);
    PARAM_SET_FLOAT(INS, INS_ACC3OFFS_Z, _imu->_accel_offset[2].z);
    
    PARAM_SET_INT32(INS, INS_ACC_ID, _imu->_accel_id[0]);
    PARAM_SET_INT32(INS, INS_ACC2_ID, _imu->_accel_id[1]);
    PARAM_SET_INT32(INS, INS_ACC3_ID, _imu->_accel_id[2]);
    
    param_save();
}

/*------------------------------------test------------------------------------*/


